#include "fake_odom/FakeOdometry.h"
#include <random>
#include <fstream>  
#include <iostream>
#include <string>
#include <sstream>

std::string filename = std::string(std::getenv("HOME")) + std::string("/map/init_pose.txt");

FakeOdometry::FakeOdometry()
: Node("fake_odom"),
  cmd_vel_{},
  static_noise_level_(0.0),
  move_noise_level_(0.0),
  yaw_noise_level_(0.0)
{
  // 声明和获取参数
  this->declare_parameter("static_noise_level", 0.0);
  this->declare_parameter("move_noise_level", 0.0);
  this->declare_parameter("yaw_noise_level", 0.0);
  this->declare_parameter("cmdvel_topic", "/cmd_vel");
  this->declare_parameter("initialpose_topic", "/initialpose");
  this->declare_parameter("odom_topic", "/odom_fake");
  this->declare_parameter("baselink_frame", "base_link");
  this->declare_parameter("odom_frame", "odom");
  this->declare_parameter("map_frame", "map");
  this->declare_parameter("odom_pub_rate", 50.0);
  this->declare_parameter("enable_map2odom", true);

  // 获取参数
  static_noise_level_ = this->get_parameter("static_noise_level").as_double();
  move_noise_level_ = this->get_parameter("move_noise_level").as_double();
  yaw_noise_level_ = this->get_parameter("yaw_noise_level").as_double();
  cmdvel_topic_ = this->get_parameter("cmdvel_topic").as_string();
  initialpose_topic_ = this->get_parameter("initialpose_topic").as_string();
  odom_topic_ = this->get_parameter("odom_topic").as_string();
  baselink_frame_ = this->get_parameter("baselink_frame").as_string();
  odom_frame_ = this->get_parameter("odom_frame").as_string();
  map_frame_ = this->get_parameter("map_frame").as_string();
  odom_pub_rate_ = this->get_parameter("odom_pub_rate").as_double();
  enable_map2odom_ = this->get_parameter("enable_map2odom").as_bool();

  RCLCPP_INFO(this->get_logger(), "cmdvel_topic: %s", cmdvel_topic_.c_str());
  RCLCPP_INFO(this->get_logger(), "initialpose_topic: %s", initialpose_topic_.c_str());
  RCLCPP_INFO(this->get_logger(), "odom_topic: %s", odom_topic_.c_str());
  RCLCPP_INFO(this->get_logger(), "baselink_frame: %s", baselink_frame_.c_str());
  RCLCPP_INFO(this->get_logger(), "odom_frame: %s", odom_frame_.c_str());
  RCLCPP_INFO(this->get_logger(), "map_frame: %s", map_frame_.c_str());
  RCLCPP_INFO(this->get_logger(), "odom_pub_rate: %.2f", odom_pub_rate_);
  RCLCPP_INFO(this->get_logger(), "enable_map2odom: %s", enable_map2odom_ ? "true" : "false");
}

bool FakeOdometry::init()
{
   // 初始化 transform handler
  tf_ = std::make_shared<utils::TransformHandler>(shared_from_this());

  // 创建订阅者、发布者和定时器
  sub_cmdvel_ = this->create_subscription<geometry_msgs::msg::Twist>(
    cmdvel_topic_, 1, std::bind(&FakeOdometry::cmdVelCallback, this, std::placeholders::_1));

  sub_initialpose_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
    initialpose_topic_, 1, std::bind(&FakeOdometry::setInitialPose, this, std::placeholders::_1));

  pub_odom_ = this->create_publisher<agv_msgs::msg::OdometryLite>(odom_topic_, 1);

  pub_odom_timer_ = this->create_wall_timer(
    std::chrono::duration<double>(1.0 / odom_pub_rate_),
    std::bind(&FakeOdometry::deadReckoning, this));

  initPose();
  return true;
}

void FakeOdometry::initPose()
{
  double x, y, theta;
  std::ifstream inFile(filename); 

  if (!inFile) {
    RCLCPP_ERROR(this->get_logger(), "Can't open file: %s", filename.c_str());
    return;
  }
  
  std::string line;
  while (std::getline(inFile, line)) {
    std::istringstream iss(line);
    std::string label;
    char comma1, comma2;
    
    if (iss >> label >> x >> comma1 >> y >> comma2 >> theta) {
      RCLCPP_INFO(this->get_logger(), "Pose 2D: %.3f, %.3f, %.3f", x, y, theta);
    } else {
      RCLCPP_ERROR(this->get_logger(), "File error!");
    }
  }

  inFile.close();
  robot_.setPose2D(Eigen::Vector3d(x, y, theta));
}

void FakeOdometry::backUpPose()
{
  std::ofstream outFile(filename, std::ios::out | std::ios::trunc);
  
  if (!outFile) {
    RCLCPP_ERROR(this->get_logger(), "Can't open file: %s", filename.c_str());
    return;
  }

  const auto& pose_2d = robot_.getPose2D();
  outFile << "Pose " << pose_2d.x() << ", " << pose_2d.y() << ", " << pose_2d.z() << std::endl;
  outFile.close();
}

void FakeOdometry::cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
{
  cmd_vel_ = *msg;
}

void FakeOdometry::setInitialPose(
  const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
{
  geometry_msgs::msg::PoseWithCovarianceStamped initialpose_in_odom;

  auto [has_transform, transform] = tf_->getTransform(msg->header.frame_id, odom_frame_);
  if (!has_transform || !utils::tf::doTransform(*msg, initialpose_in_odom, transform)) {
    RCLCPP_INFO(this->get_logger(), 
      "Transform from %s to %s is not valid. Skip setting initial pose",
      msg->header.frame_id.c_str(), odom_frame_.c_str());
    return;
  }

  auto [roll, pitch, initial_pose_yaw] = utils::tf::getRPYFrom(
    initialpose_in_odom.pose.pose.orientation);
  const auto& initial_pose_x = initialpose_in_odom.pose.pose.position.x;
  const auto& initial_pose_y = initialpose_in_odom.pose.pose.position.y;
  
  robot_.setPose2D(Eigen::Vector3d(initial_pose_x, initial_pose_y, initial_pose_yaw));
  
  RCLCPP_INFO(this->get_logger(), "Initial pose: %.3f, %.3f, %.3f",
    initial_pose_x, initial_pose_y, initial_pose_yaw);

  cmd_vel_ = geometry_msgs::msg::Twist();
  backUpPose();
}

void FakeOdometry::deadReckoning()
{
  const auto& vx = cmd_vel_.linear.x;
  const auto& vy = cmd_vel_.linear.y;
  const auto& w = cmd_vel_.angular.z;
  Eigen::Vector3d cmd_vel(vx, vy, w);
   
  std::random_device rd;
  std::mt19937 gen(rd());
  std::normal_distribution<double> pos_noise_dist(0.0, move_noise_level_);
  std::normal_distribution<double> static_noise_dist(0.0, static_noise_level_);
  std::normal_distribution<double> yaw_noise_dist(0.0, yaw_noise_level_);

  bool move = fabs(cmd_vel_.linear.x) > 0.1 || fabs(cmd_vel_.linear.y) > 0.1;
  double pose_noise = move ? pos_noise_dist(gen) : static_noise_dist(gen);
  double yaw_noise = fabs(cmd_vel_.angular.z) > 0.01 ? 
    yaw_noise_dist(gen) : (move ? yaw_noise_dist(gen) : 0.0);

  if (fabs(pose_noise) > 0.001 || fabs(yaw_noise) > 0.001) {
    RCLCPP_INFO(this->get_logger(), "Noise level: (%.3f : %.3f : %.3f)",
      move_noise_level_, pose_noise, yaw_noise);
  }

  robot_.deadReckoning(cmd_vel, 1.0 / odom_pub_rate_, pose_noise, yaw_noise);
  const auto& pose_2d = robot_.getPose2D();

  // Publish odom and tf
  agv_msgs::msg::OdometryLite msg_odom;
  toOdomMsg(pose_2d, msg_odom);
  pub_odom_->publish(msg_odom);

  geometry_msgs::msg::Transform transform;
  toTransformMsg(pose_2d, transform);
  tf_->sendTransform(transform, odom_frame_, baselink_frame_, this->now());

  if (enable_map2odom_) {
    geometry_msgs::msg::Transform transform;
    toTransformMsg(Eigen::Vector3d(0, 0, 0), transform);
    tf_->sendTransform(transform, map_frame_, odom_frame_, this->now());
  }
}

void FakeOdometry::toOdomMsg(
  const Eigen::Vector3d& pose2D, agv_msgs::msg::OdometryLite& msg)
{
  msg.header.frame_id = odom_frame_;
  msg.header.stamp = this->now();
  msg.child_frame_id = baselink_frame_;

  msg.pose.position.x = pose2D.x();
  msg.pose.position.y = pose2D.y();
  msg.pose.position.z = 0;

  msg.pose.orientation = utils::tf::getQuaternionMsgFrom(0, 0, pose2D.z());
  msg.twist = cmd_vel_;
}

void FakeOdometry::toTransformMsg(
  const Eigen::Vector3d& pose2D, geometry_msgs::msg::Transform& transform)
{
  transform.translation.x = pose2D.x();
  transform.translation.y = pose2D.y();
  transform.rotation = utils::tf::getQuaternionMsgFrom(0, 0, pose2D.z());
}